#pragma once

#include <opencv2/opencv.hpp>

// 卡尔曼滤波预测器
class Predictor {
private:
  cv::KalmanFilter kf;
  cv::Mat state;  // [x, y, z, vx, vy, vz]
  cv::Mat meas;   // [z_x, z_y, z_z]
  bool init;      // 是否初始化
  int missing_count; // 当前丢失次数
  double delta_t; // 时间间隔

public:
  Predictor(double delta_t = 0.1) : delta_t(delta_t) {}
  // 设置时间间隔
  void setDeltaT(double dt);
  // 初始化卡尔曼滤波器
  void initKalmanFilter(const cv::Point3f &pt);
  // 预测下一位置
  cv::Point3f predict();
  // 用新的测量值更新卡尔曼滤波器
  cv::Point3f correct(const cv::Point3f &pt);
  // 获取当前状态
  cv::Point3f getState() const;
  // 获取丢失次数
  int getMissingCount() const;
};